
#include "SDCardDriver.h"
#include <inc/lm3s9d90.h>
#include <inc/hw_ints.h>
#include <inc/hw_memmap.h>
#include <inc/hw_types.h>
#include <driverlib/interrupt.h>
#include <stdio.h>
#include <string.h>

#define QUEUESIZE 10

extern SDataFile datafile;

bool SDCardDriver::Init()
{
	FRESULT fresult;
	m_FileValid = false;

	// mount
	fresult = f_mount(0, &m_sFatFs);

	if(fresult != FR_OK)
	{
		return false;
	}

	// find first file (1...100)
	for(int i=0; i!=100; i++)
	{
		char filename[100];
		sprintf(filename, "0:file-%d.txt", i);
		FRESULT res = f_open(&m_sFileObject, filename, FA_CREATE_NEW | FA_WRITE );
		if( res == FR_OK)
		{
			// new file created, OK
			m_FileValid = true;

			char data[] = "MissionTime MPUGyroX MPUGyroY MPUGyroZ MPUAccX MPUAccY MPUAccZ MPUTemp GyroX GyroY GyroZ AccX AccY AccZ Roll Pitch Yaw MagX MagY MagZ Voltage Current Power PWM1 PWM2 PWM3 PWM4 Longitude Latitude GPSAlt SatCnt Temp AbsPress DiffPress Sonar IRNear IRFar XBeeTime XBeeLoopMS FailedQueues AUX1 AUX2 AUX3 AUX4 AUX5 AUX6 AUX7 AUX8 AUX9 AUX10\r\n";
			WriteData(data, strlen(data)); // add header->easy matlab parsing

			return true;
		}
		if( res != FR_EXIST) return false; // unknown error!!!
	}

	m_DataQueue.clear();

	return false; // ERROR
}

int sizeSDWrite = 0;
bool SDCardDriver::WriteData(char* data, int len)
{
	if( !m_FileValid ) return false;

	// write file
	WORD bWr;
	FRESULT res =f_write(&m_sFileObject,data,len,&bWr);
	if( res != FR_OK) return false;

	sizeSDWrite = len;

	return true;
}

bool SDCardDriver::Flush()
{
	if( !m_FileValid ) return false;

	f_sync(&m_sFileObject);

	return true;
}

bool SDCardDriver::ChunkData()
{
	if( m_DataQueue.size() >= QUEUESIZE)
	{
		datafile.FailedQueues++;
		return false; // no more space
	}

	IntMasterDisable();
	SDataFile df;
	memcpy(&df, &datafile, sizeof(datafile));
	m_DataQueue.push_back(df);
	IntMasterEnable();

	return true;
}

bool SDCardDriver::WriteChunks()
{
	// write all chunks
	while( !m_DataQueue.empty() )
	{
		// get chunk
		IntMasterDisable();
		SDataFile df = m_DataQueue.front();
		m_DataQueue.pop_front();
		IntMasterEnable();

		// XXX - Write data to card
		char data[1000];
		//"MissionTime MPUGyroX MPUGyroY MPUGyroZ MPUAccX MPUAccY MPUAccZ MPUTemp GyroX GyroY GyroZ AccX AccY AccZ Roll Pitch Yaw MagX MagY MagZ Voltage Current Power PWM1 PWM2 PWM3 PWM4 Longitude Latitude GPSAlt SatCnt Temp AbsPress DiffPress Sonar IRNear IRFar XBeeTime XBeeLoopMS FailedQueues AUX1 AUX2 AUX3 AUX4 AUX5 AUX6 AUX7 AUX8 AUX9 AUX10\r\n";
		sprintf(data, "%0.3lf %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.2f", df.MissionTime, df.MPUGyroX, df.MPUGyroY, df.MPUGyroZ, df.MPUAccX, df.MPUAccY, df.MPUAccZ, df.MPUTemperature );
		sprintf(data, "%s %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", data, df.gyroRollX, df.gyroPitchY, df.gyroYawZ, df.accX, df.accY, df.accZ  );
		sprintf(data, "%s %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", data, df.Roll, df.Pitch, df.Yaw, df.magX, df.magY, df.magZ  );
		sprintf(data, "%s %0.2f %0.2f %0.2f", data, df.Voltage, df.Current, df.Power );
		sprintf(data, "%s %0.1f %0.1f %0.1f %0.1f", data, df.PWMOut1, df.PWMOut2, df.PWMOut3, df.PWMOut4 );
		sprintf(data, "%s %0.1lf %0.1lf %0.3f %d", data, df.gpsLon, df.gpsLat, df.gpsAlt, df.gpsSatCount );
		sprintf(data, "%s %0.1f %0.3f %0.3f", data, df.Temperature, df.AbsPressure, df.DiffPressure );
		sprintf(data, "%s %0.3f %0.3f %0.3f", data, df.Sonar, df.IRNear, df.IRFar );
		sprintf(data, "%s %0.3lf %0.3f %d", data, df.XBeeTotalTime, df.XBeeLoopTimeMS, df.FailedQueues );
		sprintf(data, "%s %0.3f %0.3f %0.3f %0.3f %0.3f", data, df.ExtraData[0], df.ExtraData[1], df.ExtraData[2],df.ExtraData[3], df.ExtraData[4] );
		sprintf(data, "%s %0.3f %0.3f %0.3f %0.3f %0.3f", data, df.ExtraData[5], df.ExtraData[6], df.ExtraData[7],df.ExtraData[8], df.ExtraData[9] );
		sprintf(data, "%s\r\n", data );
		WriteData(data, strlen(data)); // write to card
	}

	return true;
}
